#include "plane_header.h"
#include <Arduino.h>
#include <Wire.h>
#include "BluetoothSerial.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

// 函数定义
void Motor_Drive(int signal_PWM_1, int signal_PWM_2, int signal_PWM_3, int signal_PWM_4)
{
  ledcWrite(PWM1_pin, signal_PWM_1);
  ledcWrite(PWM2_pin, signal_PWM_2);
  ledcWrite(PWM3_pin, signal_PWM_3);
  ledcWrite(PWM4_pin, signal_PWM_4);
}

void Emergency_Check(float ypr[3])
{
  //ypr yaw,pitch,roll,偏航角,俯仰角，滚转角
  float a_roll, a_pitch, a_yaw;

  a_roll = ypr[2] * 180 / M_PI;
  a_pitch = ypr[1] * 180 / M_PI;
  a_yaw = ypr[0] * 180 / M_PI;

  if (abs(a_roll) > EMERGENCY_THRESHOLD || abs(a_pitch) > EMERGENCY_THRESHOLD)
  {
    Motor_Drive(0, 0, 0, 0);
    SerialBT.print("Battery=");
    SerialBT.print(float(analogReadMilliVolts(AD_pin)) * 0.0476);
    SerialBT.println("\%");

    SerialBT.print("ypr\t");
    SerialBT.print(ypr[0] * 180 / PI);
    SerialBT.print("\t");
    SerialBT.print(ypr[1] * 180 / PI);
    SerialBT.print("\t");
    SerialBT.println(ypr[2] * 180 / PI);

    SerialBT.println("Motor Stopped due to Rollover!!! Send charactor s to continue ...");
    // while (SerialBT.read() != -1);
    // while (!SerialBT.available());
    while(1){
      if(SerialBT.available()){
        String command = SerialBT.readString();
        // 回复
        if(command == "s"){
          break;
        }
        // 改变PWM
        ud_command(command);
        // 改变pid
        change_PID(command);
      }
      // 延时，防止过载
      delay(10);
    }
    // Preheat();
  }

  //  SerialBT.print("ypr\t");
  //  SerialBT.print(a_yaw);
  //  SerialBT.print("\t");
  //  SerialBT.print(a_pitch);
  //  SerialBT.print("\t");
  //  SerialBT.println(a_roll);
}

// 角速度环（扩展，暂时用不上）
// float* calculate_pid_omega(int16_t xyz[3], float target[3])
// {
//   // 这里的ypr 分别为PCB上的绕x，y，z轴旋转的角速度
//   // 注意 方向和ypr正方向不一样
//   static float e_r_pre, e_p_pre, e_y_pre;
//   static float e_r_intg, e_y_intg, e_p_intg = 0;
//   float e_r, e_p, e_y, e_r_diff, e_p_diff, e_y_diff;
//   float PID_roll, PID_pitch, PID_yaw;
//   static float PID_omega[3];
//   if(counter == 0){
//     e_r_pre = 0;
//     e_p_pre = 0;
//     e_y_pre = 0;
//   }

//   e_r = -xyz[0] * PI/180  + target[0];
//   e_r_diff = e_r - e_r_pre;
//   e_r_intg = e_r_intg + e_r;
//   e_r_intg = constrain(e_r_intg, -E_RP_B, E_RP_B);

//   e_p = xyz[1]* PI/180 + target[1];
//   e_p_diff = e_p - e_p_pre;
//   e_p_intg = e_p_intg + e_p;
//   e_p_intg = constrain(e_p_intg, -E_RP_B, E_RP_B);

//   e_y = ypr[0]* PI/180 + target[2];
//   if (e_y > PI)
//   {
//     e_y = e_y - 2*PI;
//   }
//   else if (e_y < -PI)
//   {
//     e_y = e_y + 2*PI;
//   }
//   e_y_diff = e_y - e_y_pre;
//   if (e_y_diff > PI)
//   {
//     e_y_diff = e_y_diff - 2*PI;
//   }
//   else if (e_y_diff < -PI)
//   {
//     e_y_diff = e_y_diff + 2*PI;
//   }

//   e_y_intg = e_y_intg + e_y;
//   e_y_intg = constrain(e_y_intg, -E_Y_B, E_Y_B);

//   PID_roll = P_roll_w * e_r + I_roll_w * e_r_intg + D_roll_w * e_r_diff;
//   PID_pitch = P_pitch_w * e_p + I_pitch_w * e_p_intg + D_pitch_w * e_p_diff;
//   PID_yaw = P_yaw_w * e_y + I_yaw_w * e_y_intg + D_yaw_w * e_y_diff;
//   PID_omega[0] = PID_roll;
//   PID_omega[1] = PID_pitch;
//   PID_omega[2] = PID_yaw;
  

//   e_r_pre = e_r;
//   e_p_pre = e_p;
//   e_y_pre = e_y;
//   return PID_omega;
// }

// 位置环（内有角速度环接口）
void calculate_pid(float ypr[3], float ypr_target[3], int16_t gyro[3])
{
  // 通过PD维持四旋翼无人机的姿态
  //（PID 无 I，且考虑roll angle, pitch angle. 暂时不考虑yaw angle)
  
  // 旧的角度偏差（roll, pitch）
  static float e_r_pre, e_p_pre, e_y_pre;
  static float e_r_intg, e_y_intg, e_p_intg = 0;
  // 角度偏差 and 角度偏差变化量
  float e_r, e_p, e_y, e_r_diff, e_p_diff, e_y_diff;
  // PID 计算值
  float PID_roll, PID_pitch, PID_yaw;
  // for 串级PID
  float PID_w_i[3];
  float* PID_w_o;
  
  // 初始状态，初始化旧偏移量，并且退出
  if(counter == 0){
    e_r_pre = ypr[2] - ypr_target[2];
    e_p_pre = ypr[1] - ypr_target[1];
    e_y_pre = ypr[0] - ypr_target[0];
    return;
  }
  // 计算：偏离标准角度的量
  e_r = ypr[2] - ypr_target[2];
  // 计算：偏离量的变化量
  e_r_diff = e_r - e_r_pre;
  e_r_intg = e_r_intg + e_r;
  e_r_intg = constrain(e_r_intg, -E_RP_B, E_RP_B);

  e_p = ypr[1] - ypr_target[1];
  e_p_diff = e_p - e_p_pre;
  e_p_intg = e_p_intg + e_p;
  e_p_intg = constrain(e_p_intg, -E_RP_B, E_RP_B);

  e_y = ypr[0] - ypr_target[0];
  if (e_y > PI)
  {
    e_y = e_y - 2*PI;
  }
  else if (e_y < -PI)
  {
    e_y = e_y + 2*PI;
  }
  e_y_diff = e_y - e_y_pre;
  if (e_y_diff > PI)
  {
    e_y_diff = e_y_diff - 2*PI;
  }
  else if (e_y_diff < -PI)
  {
    e_y_diff = e_y_diff + 2*PI;
  }

  e_y_intg = e_y_intg + e_y;
  e_y_intg = constrain(e_y_intg, -E_Y_B, E_Y_B);

  PID_roll = P_roll * e_r + I_roll * e_r_intg + D_roll * tau_inverse * e_r_diff;
  PID_pitch = P_pitch * e_p + I_pitch * e_p_intg + D_pitch * tau_inverse * e_p_diff;
  PID_yaw = P_yaw * e_y + I_yaw * e_y_intg + D_yaw * tau_inverse * e_y_diff;
  // PID_w_i[0] = PID_roll;
  // PID_w_i[1] = PID_pitch;
  // PID_w_i[2] = PID_yaw;
  // PID_w_o = calculate_pid_omega(gyro, PID_w_i);

  // 双环
  // signal_PWM_1 = signal_first_PWM_1 + int(*PID_w_o) + int(*(PID_w_o+1)) - int(*(PID_w_o+2));
  // signal_PWM_2 = signal_first_PWM_2 + int(*PID_w_o) - int(*(PID_w_o+1)) + int(*(PID_w_o+2));
  // signal_PWM_3 = signal_first_PWM_3 - int(*PID_w_o) + int(*(PID_w_o+1)) + int(*(PID_w_o+2));
  // signal_PWM_4 = signal_first_PWM_4 - int(*PID_w_o) - int(*(PID_w_o+1)) - int(*(PID_w_o+2));
  // 单环
  // signal_first_PWM_1，signal_first_PWM_2，signal_first_PWM_3，signal_first_PWM_4分别为基准PWM（200）
  // 他们分别控制，右下，右上，左下，左上。
  signal_PWM_1 = signal_first_PWM_1 + (PID_roll) + (PID_pitch) - int(PID_yaw);
  signal_PWM_2 = signal_first_PWM_2 + (PID_roll) - (PID_pitch) + int(PID_yaw);
  signal_PWM_3 = signal_first_PWM_3 - (PID_roll) + (PID_pitch) + int(PID_yaw);
  signal_PWM_4 = signal_first_PWM_4 - (PID_roll) - (PID_pitch) - int(PID_yaw);


  signal_PWM_1 = constrain(int(signal_PWM_1), PWM_LB, PWM_UB);
  signal_PWM_2 = constrain(int(signal_PWM_2), PWM_LB, PWM_UB);
  signal_PWM_3 = constrain(int(signal_PWM_3), PWM_LB, PWM_UB);
  signal_PWM_4 = constrain(int(signal_PWM_4), PWM_LB, PWM_UB);

  e_r_pre = e_r;
  e_p_pre = e_p;
  e_y_pre = e_y;
}

// 打印
void UART_process()
{
  if (counter % 100 == 0)
  {
    SerialBT.println(counter);
  }
  if (counter == INTERVAL)
  {
    Motor_Drive(0, 0, 0, 0); //停飞

    for (int k = 0; k < INTERVAL; k++)
    {
      SerialBT.print("ypr:");
      SerialBT.print("(");
      SerialBT.print(y_log[k] * 180 / PI);
      SerialBT.print(","); 
      SerialBT.print(p_log[k] * 180 / PI);
      SerialBT.print(",");
      SerialBT.print(r_log[k] * 180 / PI);
      SerialBT.println(")");
    }

    SerialBT.println("PID (yaw,pitch,roll) was:");
    SerialBT.print("(");
    SerialBT.print(P_yaw);
    SerialBT.print(",");
    SerialBT.print(I_yaw);
    SerialBT.print(",");
    SerialBT.print(D_yaw);
    SerialBT.print("),");

    SerialBT.print("(");
    SerialBT.print(P_pitch);
    SerialBT.print(",");
    SerialBT.print(I_pitch);
    SerialBT.print(",");
    SerialBT.print(D_pitch);
    SerialBT.print("),");

    SerialBT.print("(");
    SerialBT.print(P_roll);
    SerialBT.print(",");
    SerialBT.print(I_roll);
    SerialBT.print(",");
    SerialBT.print(D_roll);
    SerialBT.println("),");

    SerialBT.println("Speed was:");
    SerialBT.print("(");
    SerialBT.print(signal_first_PWM_1);
    SerialBT.print(",");
    SerialBT.print(signal_first_PWM_2);
    SerialBT.print(",");
    SerialBT.print(signal_first_PWM_3);
    SerialBT.print(",");
    SerialBT.print(signal_first_PWM_4);
    SerialBT.println(")");

    SerialBT.print("Battery=");
    SerialBT.print(float(analogReadMilliVolts(AD_pin)) * 0.0476);
    SerialBT.println("\%");

    SerialBT.print("Please Enter New PID parameters: P_y,I_y,D_y,P_p,I_p,D_p,P_r,I_r,D_r,PWM1,PWM2,PWM3,PWM4:");
    while (SerialBT.read() != -1);//清空UART FIFO

    while (1) {
      if (SerialBT.available())
      { char temp_char = SerialBT.read();
        if ( temp_char == '(')
        {
          counter = 0;
          break;
        }
        else
        {
          SerialBT.println(temp_char);
        }
      }
    }

    delay(100);
    if (SerialBT.available())
    {
      P_yaw = float(SerialBT.parseInt());
      I_yaw = float(SerialBT.parseInt());
      D_yaw = float(SerialBT.parseInt());

      P_pitch = float(SerialBT.parseInt());
      I_pitch = float(SerialBT.parseInt());
      D_pitch = float(SerialBT.parseInt());

      P_roll = float(SerialBT.parseInt());
      I_roll = float(SerialBT.parseInt());
      D_roll = float(SerialBT.parseInt());

      signal_first_PWM_1 = SerialBT.parseInt();
      signal_first_PWM_2 = SerialBT.parseInt();
      signal_first_PWM_3 = SerialBT.parseInt();
      signal_first_PWM_4 = SerialBT.parseInt();


      SerialBT.println("PID (yaw,pitch,roll) updated to");
      SerialBT.print("(");
      SerialBT.print(P_yaw);
      SerialBT.print(",");
      SerialBT.print(I_yaw);
      SerialBT.print(",");
      SerialBT.print(D_yaw);
      SerialBT.print("),");

      SerialBT.print("(");
      SerialBT.print(P_pitch);
      SerialBT.print(",");
      SerialBT.print(I_pitch);
      SerialBT.print(",");
      SerialBT.print(D_pitch);
      SerialBT.print("),");

      SerialBT.print("(");
      SerialBT.print(P_roll);
      SerialBT.print(",");
      SerialBT.print(I_roll);
      SerialBT.print(",");
      SerialBT.print(D_roll);
      SerialBT.println("),");

      SerialBT.println("Speed updated to");
      SerialBT.print("(");
      SerialBT.print(signal_first_PWM_1);
      SerialBT.print(",");
      SerialBT.print(signal_first_PWM_2);
      SerialBT.print(",");
      SerialBT.print(signal_first_PWM_3);
      SerialBT.print(",");
      SerialBT.print(signal_first_PWM_4);
      SerialBT.println(")");

      Preheat();
    }
  }
}

// 预热
void Preheat()
{
  Motor_Drive(10, 0, 0, 0);//预启动
  delay(3000);
  Motor_Drive(0, 10, 0, 0);//预启动
  delay(3000);
  Motor_Drive(0, 0, 10, 0);//预启动
  delay(3000);
  Motor_Drive(0, 0, 0, 10);//预启动
  delay(3000);
 
  // Motor_Drive(20, 20, 20, 20);
  // delay(3000); 
  // // Motor_Drive(30, 30, 30, 30);
}

void change_PID(String& command){
    // 修改指令：p/i/d + y/p/r + num    (修改PID)
    command.trim();
    //防护（字符串长度大于2才可）
    if(command.length() <= 2){
      return;
    }
    String value;
    if(command[0] == 'p'){
        if(command[1] == 'y'){
            value = command.substring(2);
            P_yaw = value.toInt();
            SerialBT.print("P_yaw:");
            SerialBT.println(P_yaw);
        }
        else if(command[1] == 'p'){
            value = command.substring(2);
            P_pitch = value.toInt();
            SerialBT.print("P_pitch:");
            SerialBT.println(P_pitch);
        }
        else if(command[1] == 'r'){
            value = command.substring(2);
            P_roll = value.toInt();
            SerialBT.print("P_roll:");
            SerialBT.println(P_roll);
        }
    }
    else if(command[0] == 'i'){
        if(command[1] == 'y'){
            value = command.substring(2);
            I_yaw = value.toInt();
            SerialBT.print("I_yaw:");
            SerialBT.println(I_yaw);
        }
        else if(command[1] == 'p'){
            value = command.substring(2);
            I_pitch = value.toInt();
            SerialBT.print("I_pitch:");
            SerialBT.println(I_pitch);
        }
        else if(command[1] == 'r'){
            value = command.substring(2);
            I_roll = value.toInt();
            SerialBT.print("I_roll:");
            SerialBT.println(I_roll);
        }
    }
    else if(command[0] == 'd'){
        if(command[1] == 'y'){
            value = command.substring(2);
            D_yaw = value.toInt();
            SerialBT.print("D_yaw:");
            SerialBT.println(D_yaw);
        }
        else if(command[1] == 'p'){
            value = command.substring(2);
            D_pitch = value.toInt();
            SerialBT.print("D_pitch:");
            SerialBT.println(D_pitch);
        }
        else if(command[1] == 'r'){
            value = command.substring(2);
            D_roll = value.toInt();
            SerialBT.print("D_roll:");
            SerialBT.println(D_roll);
        }
    }
    SerialBT.println("pid parameters has been changed to:");
    print_pid();
       
}

void ud_command(String& command){
  // 处理上升/下降命令
  command.trim();
  if (command == "u") {
    // 上升：所有PWM值 +5（不超过255）
    signal_first_PWM_1 = signal_first_PWM_1 + 5;
    signal_first_PWM_2 = signal_first_PWM_2 + 5;
    signal_first_PWM_3 = signal_first_PWM_3 + 5;
    signal_first_PWM_4 = signal_first_PWM_4 + 5;
    SerialBT.print("PWM值: ");
    SerialBT.print(signal_first_PWM_1); 
  } 
  else if (command == "d") {
    // 下降：所有PWM值 -5（不低于0）
    signal_first_PWM_1 = signal_first_PWM_1 - 5;
    signal_first_PWM_2 = signal_first_PWM_2 - 5;
    signal_first_PWM_3 = signal_first_PWM_3 - 5;
    signal_first_PWM_4 = signal_first_PWM_4 - 5;
    SerialBT.print("PWM值: ");
    SerialBT.print(signal_first_PWM_1);
  }
  else if(command == "q"){
    // quit
    Motor_Drive(0, 0, 0, 0);
    SerialBT.println("进入停止模式, 发送s退出");
    while(1){
      if(SerialBT.available()){
        String do_recover = SerialBT.readString();
        if(do_recover == "s"){
          break;
        }
        change_PID(do_recover);
      }
      delay(10);
    }
    // Preheat();
  }
  
}

void print_pid(){
  // 打印所有位置环PID参数
  SerialBT.print("P_roll:");
  SerialBT.println(P_roll  );

  SerialBT.print("I_roll:");
  SerialBT.println(I_roll  );

  SerialBT.print("D_roll:");
  SerialBT.println(D_roll  );

  SerialBT.print("P_pitch:");
  SerialBT.println(P_pitch );

  SerialBT.print("I_pitch:");
  SerialBT.println(I_pitch );

  SerialBT.print("D_pitch:");
  SerialBT.println(D_pitch );
  
  SerialBT.print("P_yaw:");
  SerialBT.println(P_yaw   );

  SerialBT.print("I_yaw:");
  SerialBT.println(I_yaw   );

  SerialBT.print("D_yaw:");
  SerialBT.println(D_yaw   );
}
